#include "flash.h"


#define FLASH_SECTOR0_BASE 0x08000000
#define FLASH_SECTOR1_BASE 0x08004000
#define FLASH_SECTOR2_BASE 0x08008000
#define FLASH_SECTOR3_BASE 0x0800C000
#define FLASH_SECTOR4_BASE 0x08010000
#define FLASH_SECTOR5_BASE 0x08020000
#define FLASH_SECTOR6_BASE 0x08040000
#define FLASH_SECTOR7_BASE 0x08060000
#define TARGET_SECTOR FLASH_SECTOR7_BASE


void flash_unlock(void) {
    if (FLASH->CR & FLASH_CR_LOCK) {
        FLASH->KEYR = FLASH_KEY1;
        FLASH->KEYR = FLASH_KEY2;
    }
}

void flash_lock(void) {
    FLASH->CR |= FLASH_CR_LOCK;
}

int flash_erase_sector(uint32_t sector_address) {
    while (FLASH->SR & FLASH_SR_BSY);
    
    if (FLASH->SR & (FLASH_SR_PGAERR | FLASH_SR_PGPERR | FLASH_SR_PGSERR | FLASH_SR_WRPERR)) {
        FLASH->SR |= (FLASH_SR_PGAERR | FLASH_SR_PGPERR | FLASH_SR_PGSERR | FLASH_SR_WRPERR);
        return -1; 
    }
    
    FLASH->CR |= FLASH_CR_SER;
    
    uint32_t sector = 0;
    if (sector_address >= FLASH_SECTOR7_BASE) sector = 7;
    else if (sector_address >= FLASH_SECTOR6_BASE) sector = 6;
    else if (sector_address >= FLASH_SECTOR5_BASE) sector = 5;
    else if (sector_address >= FLASH_SECTOR4_BASE) sector = 4;
    else if (sector_address >= FLASH_SECTOR3_BASE) sector = 3;
    else if (sector_address >= FLASH_SECTOR2_BASE) sector = 2;
    else if (sector_address >= FLASH_SECTOR1_BASE) sector = 1;
    
    FLASH->CR |= (sector << 3);
    
    FLASH->CR |= FLASH_CR_STRT;
    
    while (FLASH->SR & FLASH_SR_BSY);
    
    FLASH->CR &= ~FLASH_CR_SER;
    FLASH->CR &= ~(FLASH_CR_SNB);
    

    if (FLASH->SR & (FLASH_SR_PGAERR | FLASH_SR_PGPERR | FLASH_SR_PGSERR | FLASH_SR_WRPERR)) {
        FLASH->SR |= (FLASH_SR_PGAERR | FLASH_SR_PGPERR | FLASH_SR_PGSERR | FLASH_SR_WRPERR);
        return -1; 
    }
    
    return 0;
}

int flash_write_word(uint32_t address, uint32_t data) {
    if (address & 0x3) return -1;
    FLASH->CR &= (~FLASH_CR_PSIZE);
		FLASH->CR |= FLASH_CR_PSIZE_1;
    while (FLASH->SR & FLASH_SR_BSY);
    
    if (FLASH->SR & (FLASH_SR_PGAERR | FLASH_SR_PGPERR | FLASH_SR_PGSERR | FLASH_SR_WRPERR)) {
        FLASH->SR |= (FLASH_SR_PGAERR | FLASH_SR_PGPERR | FLASH_SR_PGSERR | FLASH_SR_WRPERR);
        return -1; 
    }
    
    FLASH->CR |= FLASH_CR_PG;
    
    *(__IO uint32_t*)address = data;
    
    while (FLASH->SR & FLASH_SR_BSY);
    
    if (FLASH->SR & (FLASH_SR_PGAERR | FLASH_SR_PGPERR | FLASH_SR_PGSERR | FLASH_SR_WRPERR)) {
        FLASH->SR |= (FLASH_SR_PGAERR | FLASH_SR_PGPERR | FLASH_SR_PGSERR | FLASH_SR_WRPERR);
        FLASH->CR &= ~FLASH_CR_PG; 
        return -1; 
    }
    
    FLASH->CR &= ~FLASH_CR_PG;
    
    return 0; 
}


uint32_t flash_read_word(uint32_t address) {
    return *(__IO uint32_t*)address;
}

void flash_erase_data(void){
	flash_unlock();
	flash_erase_sector(TARGET_SECTOR);
	flash_lock();
}

void flash_write_data(U32 addr, U32 *buff, U16 size){
	U16 i;
	flash_unlock();
	for(i = 0; i < size; i++){
		flash_write_word(TARGET_SECTOR + i * 4 + addr, buff[i]);
	}
	flash_lock();
}
void flash_read_data(U32 addr, U32 *buff, U16 size){
	U16 i;
	for(i = 0; i < size; i++){
		buff[i] = flash_read_word(TARGET_SECTOR + addr + i * 4);
	}		
}
